#include "mainwindow.h"
#include <QDebug>

// 获取速度并显示
void SpeedDataThread::run()
{

    if(!speed_data_node_cpp)
    {
        speed_data_node_cpp = rclcpp::Node::make_shared("speed_sub");
    }
    speed_data1_sub = speed_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "rt1_speed",
        10,
        std::bind(&SpeedDataThread::speed_data1_cb,this,std::placeholders::_1));

    speed_data2_sub = speed_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "rt2_speed",
        10,
        std::bind(&SpeedDataThread::speed_data2_cb,this,std::placeholders::_1));

    speed_data3_sub = speed_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "rt3_speed",
        10,
        std::bind(&SpeedDataThread::speed_data3_cb,this,std::placeholders::_1));

    speed_data4_sub = speed_data_node_cpp->create_subscription<std_msgs::msg::Int32>(
        "rt4_speed",
        10,
        std::bind(&SpeedDataThread::speed_data4_cb,this,std::placeholders::_1));

    rclcpp::spin(speed_data_node_cpp);
}


void SpeedDataThread::speed_data1_cb(const std_msgs::msg::Int32 speedmsg)
{
    float wheel1speed = speedmsg.data;
    qDebug()<<wheel1speed;
    float turespeed1 = (wheel1speed/100/101)*360*3.14/180*10;
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    turespeed1 = turespeed1*1.08 ;
    // qDebug()<<turespeed1;
    emit speedEmitData1(turespeed1);
}
void SpeedDataThread::speed_data2_cb(const std_msgs::msg::Int32 speedmsg)
{
    float wheel2speed = speedmsg.data;
    // qDebug()<<"wheel2speed";
    float turespeed2 = - (wheel2speed/100/101)*360*3.14/180*10;
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    turespeed2 = turespeed2*1.06  ;
    // qDebug()<<turespeed2;
    emit speedEmitData2(turespeed2);
}

void SpeedDataThread::speed_data3_cb(const std_msgs::msg::Int32 speedmsg)
{
    float wheel3speed = speedmsg.data;
    // qDebug()<<wheel3speed;
    float turespeed3 = (wheel3speed/100/101)*360*3.14/180*10;
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    turespeed3 = turespeed3*1.03  ;
    // qDebug()<<turespeed3;
    emit speedEmitData3(turespeed3);
}

void SpeedDataThread::speed_data4_cb(const std_msgs::msg::Int32 speedmsg)
{
    float wheel4speed = speedmsg.data;
    // qDebug()<<wheel4speed;
    float turespeed4 = (wheel4speed/100/101)*360*3.14/180*10;
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
    turespeed4 = turespeed4*1.07 ;
    // qDebug()<<turespeed4;
    emit speedEmitData4(turespeed4);
}


